import pybullet as p
import pybullet_data
import math
import numpy as np
import time
from typing import List, Tuple, Dict, Optional


class InteractiveRobotTrajectorySimulator:
    """
    简化版交互式机械臂桁轨仿真与碰撞检测接口
    保持原始代码的环境设置，支持鼠标交互控制和实时碰撞检测
    """

    def __init__(self, gui_mode: bool = True, collision_threshold: float = 0):
        """
        初始化仿真环境 - 与原始代码保持一致

        Args:
            gui_mode: 是否显示GUI界面
            collision_threshold: 碰撞检测阈值
        """
        # 连接物理引擎
        if gui_mode:
            self.physics_client = p.connect(p.GUI)
        else:
            self.physics_client = p.connect(p.DIRECT)

        p.setAdditionalSearchPath(pybullet_data.getDataPath())

        # 碰撞检测参数（固定值）
        self.collision_detection_enabled = True
        self.collision_threshold = collision_threshold

        # 初始化组件
        self.obstacle = None  # 桁轨（保持原始命名）
        self.robot_id = None
        self.box_body = None  #单独添加的滑动横梁box
        self.mount_constraint = None
        self.gantry_prismatic_joints = []
        self.robot_joints = []

        # 实时碰撞检测
        self.collision_warning_sphere = None
        self.last_collision_state = False
        self.collision_counter = 0
        self.collision_log = []

        # 交互控制
        self.joint_sliders = []
        self.gantry_sliders = []

        self._setup_scene_original()
        self._setup_interactive_controls()
        self._setup_collision_display()

    def _setup_scene_original(self):
        """按照原始代码设置仿真场景"""
        # 完全按照原始代码加载桁轨
        self.obstacle = p.loadURDF("./333333/urdf/3333331.urdf", [1.2885, 2.986, 0], [0, 0, 0, 1], useFixedBase=True)

        # 完全按照原始代码设置机械臂位置和姿态
        robot_offset_x = 0
        robot_offset_y = 0
        robot_offset_z = 0  # 障碍物顶部
        robot_position = [robot_offset_x, robot_offset_y, robot_offset_z]

        # 机械臂旋转180度（倒装）- 绕Y轴旋转180度
        robot_orientation = p.getQuaternionFromEuler([0, math.pi, 0])  # 绕Y轴旋转180度

        # 加载机械臂（倒装）
        self.robot_id = p.loadURDF("elfin10_l.urdf", robot_position, robot_orientation)

        # 完全按照原始代码获取桁轨滑动关节
        self.gantry_prismatic_joints = []
        num_gantry_joints = p.getNumJoints(self.obstacle)
        for i in range(num_gantry_joints):
            joint_info = p.getJointInfo(self.obstacle, i)
            if joint_info[2] == p.JOINT_PRISMATIC:
                self.gantry_prismatic_joints.append(i)
        print("桁轨滑动关节索引：", self.gantry_prismatic_joints)

        # 完全按照原始代码创建约束
        self.mount_constraint = p.createConstraint(
            self.obstacle, self.gantry_prismatic_joints[1], self.robot_id, -1,
            p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
            [0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 1]
        )

        # 创建BOX并附加到关节上
        box_half_extents = [0.231, 1.6, 0.09]  # 半轴长度
        box_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=box_half_extents)

        # 选择要附加的关节（这里使用第一个滑动关节）
        target_joint_index = self.gantry_prismatic_joints[0]

        # 获取关节位置信息
        joint_state = p.getJointState(self.obstacle, target_joint_index)
        joint_info = p.getJointInfo(self.obstacle, target_joint_index)

        # 计算BOX相对于关节的位置偏移
        # 这里使用你原来的坐标(1.65, -2.32, 0)相对于关节位置的偏移
        # 需要根据实际关节位置调整这些值
        box_offset_pos = [0, 0, 0]  # 相对于关节的局部位置
        box_offset_orn = p.getQuaternionFromEuler([0, 0, 0])  # 无旋转

        # 创建BOX并约束到关节
        self.box_body = p.createMultiBody(baseMass=1,  # 0表示静态物体
                                     baseCollisionShapeIndex=box_shape,
                                     basePosition=[1.2885, 2.986, 3.5],  # 初始位置会被约束覆盖
                                     baseOrientation=[0.7071, 0.7071, 0, 0])

        # 创建约束将BOX附加到关节
        box_constraint = p.createConstraint(self.obstacle, target_joint_index,
                                            self.box_body, -1,
                                            p.JOINT_FIXED,
                                            jointAxis=[0, 0, 0],
                                            parentFramePosition=box_offset_pos,
                                            childFramePosition=[-0.151, 0, 0],
                                            parentFrameOrientation=box_offset_orn,
                                            childFrameOrientation=[0, 0, 0, 1])


        # 获取机械臂关节
        self._get_robot_joints()

        print(f"机械臂关节数量: {len(self.robot_joints)}")

    def _get_robot_joints(self):
        """获取机械臂的旋转关节"""
        self.robot_joints = []
        num_joints = p.getNumJoints(self.robot_id)
        for i in range(num_joints):
            joint_info = p.getJointInfo(self.robot_id, i)
            if joint_info[2] == p.JOINT_REVOLUTE:
                self.robot_joints.append(i)

    def _setup_interactive_controls(self):
        """设置交互式控制界面"""
        # 创建机械臂关节控制滑条
        self.joint_sliders = []
        for i in range(min(6, len(self.robot_joints))):  # 限制为6个关节
            joint_info = p.getJointInfo(self.robot_id, self.robot_joints[i])
            joint_name = joint_info[1].decode('utf-8')
            lower_limit = joint_info[8]
            upper_limit = joint_info[9]

            slider_id = p.addUserDebugParameter(
                f"Joint {i + 1}: {joint_name}",
                lower_limit, upper_limit, 0
            )
            self.joint_sliders.append(slider_id)

        # 创建桁轨控制滑条
        if len(self.gantry_prismatic_joints) >= 2:
            # Y方向滑动关节
            y_slider = p.addUserDebugParameter("桁轨 Y 位置", 0, 6, 0)
            # X方向滑动关节
            x_slider = p.addUserDebugParameter("桁轨 X 位置", 0, 2.6, 0)
            self.gantry_sliders = [y_slider, x_slider]

        print("交互控制界面已设置:")
        print("- 使用滑条控制机械臂关节和桁轨位置")
        print(f"- 实时碰撞检测已开启，阈值: {self.collision_threshold}")

    def _setup_collision_display(self):
        """设置碰撞显示组件"""
        # 创建碰撞警告球体（初始隐藏）
        self.collision_warning_sphere = None

        # 添加碰撞状态显示文本
        self.collision_text_id = p.addUserDebugText(
            "碰撞状态: 无碰撞",
            [0, 0, 3],
            textColorRGB=[0, 1, 0],
            textSize=2
        )

    def update_from_sliders(self):
        """根据滑条更新机械臂和桁轨位置"""
        # 更新机械臂关节
        joint_angles = []
        for i, slider_id in enumerate(self.joint_sliders):
            angle = p.readUserDebugParameter(slider_id)
            joint_angles.append(angle)
            if i < len(self.robot_joints):
                p.setJointMotorControl2(
                    self.robot_id, self.robot_joints[i],
                    p.POSITION_CONTROL, targetPosition=angle
                )

        # 更新桁轨位置
        gantry_positions = []
        if len(self.gantry_sliders) >= 2:
            y_pos = p.readUserDebugParameter(self.gantry_sliders[0])
            x_pos = p.readUserDebugParameter(self.gantry_sliders[1])
            gantry_positions = [x_pos, y_pos]

            # 按照原始代码的方式设置桁轨位置
            p.setJointMotorControl2(
                self.obstacle, self.gantry_prismatic_joints[0],
                p.POSITION_CONTROL, targetPosition=y_pos
            )
            p.setJointMotorControl2(
                self.obstacle, self.gantry_prismatic_joints[1],
                p.POSITION_CONTROL, targetPosition=x_pos
            )

        return joint_angles, gantry_positions

    def get_end_effector_pose(self) -> Tuple[List[float], List[float]]:
        """获取末端执行器的位置和姿态"""
        # 获取末端链接的状态
        link_state = p.getLinkState(self.robot_id, len(self.robot_joints))
        position = list(link_state[0])
        orientation = list(link_state[1])
        return position, orientation

    def check_collision(self) -> bool:
        """检查是否发生碰撞"""
        # 检查机械臂与桁轨的碰撞
        contact_points01 = p.getContactPoints(self.robot_id)  #检测
        # contact_points02 = p.getContactPoints(self.robot_id, self.box_body)

        # 过滤掉约束连接点的接触
        collision_detected = False
        for contact in contact_points01:
            # 如果接触距离小于阈值，认为是碰撞
            if contact[8] < self.collision_threshold:  # contact[8] 是接触距离
                collision_detected = True
                break
        # for contact in contact_points02:
        #     # 如果接触距离小于阈值，认为是碰撞
        #     if contact[8] < self.collision_threshold:  # contact[8] 是接触距离
        #         collision_detected = True
        #         break
        return collision_detected

    def get_collision_info(self) -> List[Dict]:
        """获取详细的碰撞信息"""
        contact_points = p.getContactPoints(self.robot_id, self.obstacle)
        collision_info = []

        for contact in contact_points:
            if contact[8] < self.collision_threshold:  # 碰撞阈值
                info = {
                    'contact_distance': contact[8],
                    'contact_point_A': contact[5],
                    'contact_point_B': contact[6],
                    'contact_normal': contact[7],
                    'link_A': contact[3],
                    'link_B': contact[4],
                    'normal_force': contact[9]
                }
                collision_info.append(info)

        return collision_info

    def update_collision_display(self):
        """更新碰撞显示"""
        if not self.collision_detection_enabled:
            return False

        # 检查碰撞
        collision_detected = self.check_collision()
        collision_info = self.get_collision_info()

        # 获取末端执行器位置
        ee_position, _ = self.get_end_effector_pose()

        if collision_detected:
            # 碰撞发生
            if not self.last_collision_state:
                # 刚刚发生碰撞
                self.collision_counter += 1
                collision_log_entry = {
                    'time': time.time(),
                    'counter': self.collision_counter,
                    'ee_position': ee_position.copy(),
                    'collision_info': collision_info.copy()
                }
                self.collision_log.append(collision_log_entry)

                print(f"🚨 碰撞 #{self.collision_counter} 检测到！")
                print(f"   末端位置: {[round(x, 3) for x in ee_position]}")
                if collision_info:
                    print(f"   碰撞距离: {collision_info[0]['contact_distance']:.4f}")
                    print(f"   接触力: {collision_info[0]['normal_force']:.2f}")

            # 显示红色警告球体在末端执行器位置
            if self.collision_warning_sphere is not None:
                try:
                    p.removeBody(self.collision_warning_sphere)
                except:
                    pass

            # 创建红色警告球体
            sphere_visual = p.createVisualShape(
                p.GEOM_SPHERE,
                radius=0.05,
                rgbaColor=[1, 0, 0, 0.8]
            )
            self.collision_warning_sphere = p.createMultiBody(
                baseMass=0,
                baseVisualShapeIndex=sphere_visual,
                basePosition=ee_position
            )

            # 更新碰撞状态文本
            collision_text = f"🚨 碰撞检测到! #{self.collision_counter}"
            if collision_info:
                collision_text += f" | 距离: {collision_info[0]['contact_distance']:.3f}"

            p.addUserDebugText(
                collision_text,
                [0, 0, 3],
                textColorRGB=[1, 0, 0],
                textSize=2,
                replaceItemUniqueId=self.collision_text_id
            )

        else:
            # 无碰撞
            if self.last_collision_state:
                # 刚刚脱离碰撞
                print(f"✅ 碰撞解除")

            # 移除警告球体
            if self.collision_warning_sphere is not None:
                try:
                    p.removeBody(self.collision_warning_sphere)
                except:
                    pass
                self.collision_warning_sphere = None

            # 更新状态文本
            p.addUserDebugText(
                f"✅ 无碰撞 | 阈值: {self.collision_threshold:.3f} | 总碰撞: {self.collision_counter}",
                [0, 0, 3],
                textColorRGB=[0, 1, 0],
                textSize=2,
                replaceItemUniqueId=self.collision_text_id
            )

        # 更新碰撞状态
        self.last_collision_state = collision_detected

        return collision_detected

    def simulate_trajectory(self, trajectory: List[Tuple[List[float], float, float]],
                            step_time: float = 0.01,
                            real_time: bool = True) -> Dict:
        """
        播放预定义轨迹

        Args:
            trajectory: 轨迹列表，每个元素为(joint_angles, x_pos, y_pos)
            step_time: 每步的时间间隔
            real_time: 是否实时显示

        Returns:
            仿真结果字典
        """
        results = {
            'trajectory_positions': [],
            'collision_detected': False,
            'collision_steps': [],
            'collision_info': []
        }

        print(f"开始播放轨迹，共 {len(trajectory)} 个点...")
        print("绿色线条: 正常轨迹 | 红色线条: 碰撞段")

        trajectory_line_ids = []
        trajectory_points = []

        for step, (joint_angles, x_pos, y_pos) in enumerate(trajectory):
            # 设置桁轨位置
            p.setJointMotorControl2(
                self.obstacle, self.gantry_prismatic_joints[0],
                p.POSITION_CONTROL, targetPosition=y_pos
            )
            p.setJointMotorControl2(
                self.obstacle, self.gantry_prismatic_joints[1],
                p.POSITION_CONTROL, targetPosition=x_pos
            )

            # 设置机械臂关节
            for i, angle in enumerate(joint_angles[:6]):  # 限制为6个关节
                if i < len(self.robot_joints):
                    p.setJointMotorControl2(
                        self.robot_id, self.robot_joints[i],
                        p.POSITION_CONTROL, targetPosition=angle
                    )

            # 等待运动稳定
            for _ in range(10):
                p.stepSimulation()
                if real_time:
                    time.sleep(step_time / 10)

            # 获取末端执行器位置
            ee_position, ee_orientation = self.get_end_effector_pose()
            results['trajectory_positions'].append(ee_position)

            # 检查碰撞
            collision = self.check_collision()
            if collision:
                results['collision_detected'] = True
                results['collision_steps'].append(step)
                collision_info = self.get_collision_info()
                results['collision_info'].extend(collision_info)
                print(f"步骤 {step}: 检测到碰撞！")

            # 添加轨迹点可视化
            if collision:
                # 红色球体表示碰撞点
                sphere_visual = p.createVisualShape(
                    p.GEOM_SPHERE, radius=0.03,
                    rgbaColor=[1, 0, 0, 0.8]
                )
            else:
                # 蓝色小球表示正常轨迹点
                sphere_visual = p.createVisualShape(
                    p.GEOM_SPHERE, radius=0.015,
                    rgbaColor=[0, 0.5, 1, 0.6]
                )

            sphere_body = p.createMultiBody(
                baseMass=0, baseVisualShapeIndex=sphere_visual,
                basePosition=ee_position
            )
            trajectory_line_ids.append(sphere_body)

            # 绘制轨迹线段
            if len(trajectory_points) > 0:
                start_pos = trajectory_points[-1]
                end_pos = ee_position

                if collision:
                    # 红色线段表示碰撞段
                    line_id = p.addUserDebugLine(
                        start_pos, end_pos,
                        lineColorRGB=[1, 0, 0],
                        lineWidth=5
                    )
                else:
                    # 绿色线段表示正常轨迹
                    line_id = p.addUserDebugLine(
                        start_pos, end_pos,
                        lineColorRGB=[0, 1, 0.2],
                        lineWidth=3
                    )

                trajectory_line_ids.append(line_id)

            trajectory_points.append(ee_position)

            if real_time:
                time.sleep(step_time)

        print(f"轨迹播放完成。{'检测到碰撞' if results['collision_detected'] else '无碰撞'}")
        return results

    def clear_collision_log(self):
        """清除碰撞日志"""
        self.collision_log = []
        self.collision_counter = 0
        print("碰撞日志已清除")

    def print_collision_summary(self):
        """打印碰撞摘要"""
        if not self.collision_log:
            print("无碰撞记录")
            return

        print(f"\n=== 碰撞摘要 ===")
        print(f"总碰撞次数: {len(self.collision_log)}")

        for i, log_entry in enumerate(self.collision_log):
            print(f"碰撞 #{log_entry['counter']}:")
            print(f"  末端位置: {[round(x, 3) for x in log_entry['ee_position']]}")
            if log_entry['collision_info']:
                info = log_entry['collision_info'][0]
                print(f"  碰撞距离: {info['contact_distance']:.4f}")
                print(f"  接触力: {info['normal_force']:.2f}")

    def run_interactive_simulation(self):
        """运行交互式仿真主循环"""
        print("\n=== 交互式仿真开始 ===")
        print("使用说明:")
        print("1. 用滑条控制机械臂关节和桁轨位置")
        print("2. 实时碰撞检测：红色球体显示碰撞位置")
        print(f"3. 碰撞检测阈值: {self.collision_threshold}")
        print("4. 按Ctrl+C退出仿真")

        try:
            while True:
                # 更新机械臂和桁轨位置
                self.update_from_sliders()

                # 实时碰撞检测和显示
                self.update_collision_display()

                # 物理仿真步进
                p.stepSimulation()
                time.sleep(1. / 240.)  # 240Hz仿真频率

        except KeyboardInterrupt:
            print("\n仿真被用户中断")
            self.print_collision_summary()
        except Exception as e:
            print(f"\n仿真出错: {e}")
        finally:
            self.cleanup()

    def cleanup(self):
        """清理仿真环境"""
        self.print_collision_summary()
        p.disconnect(self.physics_client)
        print("仿真环境已清理")


def interactive_example():
    """交互式使用示例"""
    print("启动交互式机械臂仿真...")

    # 可以在这里调整碰撞检测阈值
    simulator = InteractiveRobotTrajectorySimulator(
        gui_mode=True,
        collision_threshold=-0.001  # 调整这个值来改变碰撞敏感度
    )

    # 运行交互式仿真
    simulator.run_interactive_simulation()




def trajectory_test_example():
    """从文件读取轨迹测试示例 - 修复负号读取问题"""
    print("启动轨迹测试仿真...")
    simulator = InteractiveRobotTrajectorySimulator(gui_mode=True)

    # 从文件读取轨迹数据
    trajectory_file = "0_0_pearl_wet.txt"
    print(f"正在读取轨迹文件: {trajectory_file}")

    try:
        trajectory = []
        with open(trajectory_file, 'r', encoding='utf-8') as file:
            lines = file.readlines()

        print(f"文件包含 {len(lines)} 行数据")

        # 分析第一行数据来确定格式
        if lines:
            first_line = lines[0].strip()
            print(f"第一行原始数据: {first_line}")

            # 更仔细地解析数据，确保正确处理负号
            raw_values = first_line.split(',')
            print(f"分割后的原始值: {raw_values[:10]}")  # 显示前10个原始值

            try:
                sample_data = []
                for val in raw_values:
                    val = val.strip()  # 去除空格
                    if val:  # 确保不是空字符串
                        sample_data.append(float(val))

                print(f"样本数据长度: {len(sample_data)}")
                print(f"样本数据前10个值: {sample_data[:10]}")

                # 检查是否有负数
                negative_count = sum(1 for x in sample_data[:10] if x < 0)
                print(f"前10个值中的负数个数: {negative_count}")

            except ValueError as e:
                print(f"解析第一行数据时出错: {e}")

        for line_num, line in enumerate(lines):
            line = line.strip()
            if not line:  # 跳过空行
                continue

            try:
                # 更安全的数据解析方式
                raw_values = line.split(',')
                data = []

                for val in raw_values:
                    val = val.strip()  # 去除前后空格
                    if val:  # 确保不是空字符串
                        try:
                            # 直接转换为float，保持负号
                            float_val = float(val)
                            data.append(float_val)
                        except ValueError:
                            print(f"警告: 第 {line_num + 1} 行包含无效数值: '{val}'")
                            continue

                if len(data) < 6:  # 至少需要6个关节角度
                    print(f"警告: 第 {line_num + 1} 行有效数据不足({len(data)}个)，跳过")
                    continue

                # 根据数据格式提取关节角度和桁轨位置
                # 前6个是机械臂关节角度（度）
                joint_angles_deg = data[0:6]

                # 桁轨位置 - 根据数据长度灵活处理
                if len(data) >= 8:
                    # 如果有足够数据，使用第7、8个值作为桁轨位置
                    gantry_x = data[7] / 1000.0  # 转换为米
                    gantry_y = data[6] / 1000.0  # 转换为米
                elif len(data) >= 7:
                    # 如果只有7个数据，第7个作为Y轴，X轴设为0
                    gantry_x = 0.0
                    gantry_y = data[6] / 1000.0
                else:
                    # 如果只有6个关节角度，桁轨位置设为0
                    gantry_x = 0.0
                    gantry_y = 0.0

                # 将角度从度转换为弧度（保持负数）
                joint_angles_rad = [math.radians(angle) for angle in joint_angles_deg]

                # 格式：(joint_angles, x_pos, y_pos)
                trajectory.append((joint_angles_rad, gantry_x, gantry_y))

                # 每100行打印一次进度，同时显示数据范围
                if (line_num + 1) % 100 == 0:
                    print(f"已处理 {line_num + 1} 行数据")
                    # 显示当前行的关节角度范围
                    min_angle = min(joint_angles_deg)
                    max_angle = max(joint_angles_deg)
                    print(f"  当前行关节角度范围: [{min_angle:.1f}°, {max_angle:.1f}°]")

            except (ValueError, IndexError) as e:
                print(f"警告: 第 {line_num + 1} 行数据格式错误，跳过: {e}")
                print(f"  原始数据: {line[:100]}")  # 显示前100个字符
                continue

        print(f"成功读取 {len(trajectory)} 个轨迹点")

        if len(trajectory) == 0:
            print("错误: 没有有效的轨迹数据")
            return

        # 显示前几个轨迹点的信息，重点检查负数
        print("\n前3个轨迹点预览（检查负数）:")
        for i in range(min(3, len(trajectory))):
            joints, x_pos, y_pos = trajectory[i]
            print(f"点 {i + 1}:")
            print(f"  关节角度(度): {[round(math.degrees(j), 2) for j in joints]}")
            print(f"  关节角度(弧度): {[round(j, 3) for j in joints]}")
            print(f"  桁轨X位置(米): {round(x_pos, 3)}")
            print(f"  桁轨Y位置(米): {round(y_pos, 3)}")

            # 检查负数
            negative_joints = [i for i, angle in enumerate(joints) if angle < 0]
            if negative_joints:
                print(f"  包含负角度的关节: {[i + 1 for i in negative_joints]}")
            print()

        # 统计整个轨迹中的角度范围
        all_angles_deg = []
        for joints, _, _ in trajectory:
            all_angles_deg.extend([math.degrees(angle) for angle in joints])

        if all_angles_deg:
            min_all = min(all_angles_deg)
            max_all = max(all_angles_deg)
            negative_count = sum(1 for angle in all_angles_deg if angle < 0)
            print(f"整体角度统计:")
            print(f"  角度范围: [{min_all:.1f}°, {max_all:.1f}°]")
            print(
                f"  负角度数量: {negative_count}/{len(all_angles_deg)} ({negative_count / len(all_angles_deg) * 100:.1f}%)")

        # 询问用户是否要播放轨迹
        play_choice = input(f"\n是否播放包含 {len(trajectory)} 个点的轨迹? (y/n): ").strip().lower()
        if play_choice != 'y':
            print("跳过轨迹播放")
        else:
            # 询问播放速度
            # speed_choice = input("选择播放速度 (1=正常, 2=快速, 3=慢速): ").strip()
            # if speed_choice == "2":
            #     step_time = 0  # 快速
            # elif speed_choice == "3":
            #     step_time = 0.5  # 慢速
            # else:
            #     step_time = 0.1  # 正常


            step_time = 0
            print(f"开始播放轨迹，步进时间: {step_time}s")

            # 执行轨迹
            results = simulator.simulate_trajectory(trajectory, step_time=step_time, real_time=True)

            # 分析结果
            print(f"\n=== 轨迹测试结果 ===")
            print(f"轨迹点数: {len(results['trajectory_positions'])}")
            print(f"碰撞检测: {'发生碰撞' if results['collision_detected'] else '无碰撞'}")

            if results['collision_detected']:
                print(f"碰撞步骤: {results['collision_steps']}")
                print(f"碰撞次数: {len(results['collision_steps'])}")
                collision_rate = len(results['collision_steps']) / len(trajectory) * 100
                print(f"碰撞率: {collision_rate:.1f}%")

    except FileNotFoundError:
        print(f"错误: 找不到文件 {trajectory_file}")
        print("请确保文件在当前目录下")

        # 生成示例轨迹作为后备方案
        print("使用示例轨迹...")
        trajectory = []
        for i in range(10):
            t = i / 9.0
            joint_angles = [
                0.5 * t,  # 关节1：正向旋转
                -0.5 * t,  # 关节2：负向弯曲
                1.0 * t,  # 关节3：正向弯曲
                -0.5 * t,  # 关节4：负向弯曲
                0.3 * t,  # 关节5：正向弯曲
                -0.3 * t  # 关节6：负向旋转
            ]
            x_pos = 0.3 * t  # X轴移动
            y_pos = 0.5 * t  # Y轴移动
            trajectory.append((joint_angles, x_pos, y_pos))

        # 执行示例轨迹
        results = simulator.simulate_trajectory(trajectory, step_time=0.2, real_time=True)

    except Exception as e:
        print(f"读取文件时发生错误: {e}")
        import traceback
        traceback.print_exc()  # 打印详细错误信息
        return

    # 切换到交互模式
    print("\n切换到交互控制模式...")
    simulator.run_interactive_simulation()

if __name__ == "__main__":
    print("选择运行模式：")
    print("1. 交互式控制模式")
    print("2. 轨迹测试 + 交互模式")

    choice = input("请输入选择 (1 或 2): ").strip()

    if choice == "2":
        trajectory_test_example()
    else:
        interactive_example()